// Feature.cpp: implementation of the CFeature class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "LXH_Feature.h"
#include "fstream.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CFeature::CFeature()
{
	para.SigP_RmvSilWinSz = 32;
	para.SigP_RmvSilCoef = 0.1f;
	
	para.VariationStpSz = 1;

	para.WinSz = -1;				// default 256 samples
	para.StpSz = -1;				// default 128 samples
	para.FFTSz = -1;
	para.FType;						// feature sets, bit ORed;
	para.PreEmphasis = true;		// PreEmphasis with Alpha Coef
	para.Alpha = 0.97f;				// preemphesis, default=0.97
	para.Windowing = 0;				// 0 : No windowing, HAMMING_WIN, HANNING_WIN or TRIANGLE_WIN, default : 0
	para.RmvSilence = 0;			// 1: remove silence part, 0: intact
	para.RmvDC = 0;

	para.Filtering = 0;				// 0 : no filter, LOWPASS_FIL, HIGHPASS_FIL, default : 0
	para.LowFilCutOffFreq = 5000;	//// Cut Off frquence of low filter, default : 5000
	para.HighFilCutOffFreq = 0;		
	para.FilOrder = 199;					// Order of filter, default : 199

	//======================
	// extra parameter of MFCC 
	//======================
	para.SRate = 16000;	            // sampling rate of Sig;
	//======================
	// new para for Pitch 
	//======================
	para.Smooth = 1;				// 1: smooth pitch contour with 3-point median filter

	fSignal = NULL;
	fHamming = new FilterWin;
	fHamming->CreateWin(1);
	fHamming->fWin[0] = 1.0f;
	fHanning = new FilterWin;
	fHanning->CreateWin(1);
	fHanning->fWin[0] = 1.0f;
	fTriangle = new FilterWin;
	fTriangle->CreateWin(1);
	fTriangle->fWin[0] = 1.0f;
}

CFeature::~CFeature()
{
	if( fSignal != NULL ) free( fSignal );
	if( fHamming != NULL ) 
	{
		fHamming->DeleteWin();
		delete fHamming;
	}
	if( fHanning != NULL ) 
	{
		fHanning->DeleteWin();
		delete fHanning;
	}
	if( fTriangle != NULL ) 
	{
		fTriangle->DeleteWin();
		delete fTriangle;
	}
}

void CFeature::CopySignal(short *lpSignal, int nLen, int nSampleRate)
{
	para.SRate = nSampleRate;
	if( para.WinSz < 0 ) 
		para.WinSz = int(nSampleRate * 0.01f);
	if( para.StpSz < 0 )
		para.StpSz = para.WinSz/2;

	if( fSignal != NULL ) free( fSignal );
	fSignal = (float *)calloc( nLen, sizeof(float) );
	for( int i=0; i<nLen; i++ )
		fSignal[i] = lpSignal[i];
	this->nLen = nLen;
	if( para.RmvDC ) 
		RemoveDC();
	if( para.PreEmphasis == true )
		PreEmphasis( fSignal, nLen, para.Alpha );
	if( para.RmvSilence ) RemoveSilence();

	if( para.Filtering == HIGHPASS_FIL )
		this->FIRhighpass_double_filter();
	if( para.Filtering == LOWPASS_FIL )
		this->FIRlowpass_double_filter();
	if( para.Filtering == (BANDPASS_FIL) )
	{
		this->FIRhighpass_double_filter();
		this->FIRlowpass_double_filter();
	}

	/*
	short *lp = new short[this->nLen];
	for( i = 0; i < this->nLen; i++ )
		lp[i] = fSignal[i];
	fstream f;
	f.open( "C:\\tt.dat", ios::app | ios::out | ios::binary );
	f.write( (char*)lp, this->nLen * sizeof(short));
	f.close();
	*/
}

void CFeature::GenHamWin(int nFSize)
{
	if( fHamming->nSize == nFSize ) return;
	fHamming->DeleteWin();
	fHamming->CreateWin(nFSize);

	float *r_window = fHamming->fWin;
    int i,j=nFSize/2;
    float k = float(PIx2 / nFSize);

    if( (nFSize & 1) != 0) // odd
		r_window[j]=1.0;

    for(i = 0; i < j; i++){
		r_window[i] = float(0.54 - 0.46 * cos(k * (i + 0.5)));
		r_window[nFSize-i-1] = r_window[i];
    }
}

long CFeature::RemoveSilence()
{
	if( fSignal == NULL ) ErrorMsg("Error in RemoveSilence : You must CopySignal");
	float* lpRmv;
	float EnergyThreshold = para.SigP_RmvSilCoef * MeanEnergy( fSignal, nLen );
	float E;
	int i, nNewSize;
	int nNumWins = nLen / para.SigP_RmvSilWinSz;
	if( nNumWins < 3 ) ErrorMsg("Error in RemoveSilence : Signal too short");

	BOOL	*bIsVoiced = (BOOL *)calloc( nNumWins, sizeof(BOOL) );
	for (i = 0; i < nNumWins; i++){
		if( para.SigP_RmvSilWinSz * (i+1)< nLen ){
			E = MeanEnergy( fSignal + para.SigP_RmvSilWinSz * i, para.SigP_RmvSilWinSz );
			if (E > EnergyThreshold) bIsVoiced[i] = TRUE;
		}
	}

	//Fill voiced and unvoiced parts
	for( i = 1; i < nNumWins-1; i++ )
	{
		if(( bIsVoiced[i] != bIsVoiced[i-1] ) && (bIsVoiced[i] != bIsVoiced[i+1]))
			bIsVoiced[i] = bIsVoiced[i-1];
	}
	bIsVoiced[0] = bIsVoiced[1];
	bIsVoiced[nNumWins-1] = bIsVoiced[nNumWins-2];

	
	//Eliminate the unvoiced parts
	nNewSize = 0;
	for( i = 0; i<nNumWins; i++ )
		if( bIsVoiced[i] ) nNewSize++;
	nNewSize *= para.SigP_RmvSilWinSz;
	
	lpRmv = (float*)calloc(nNewSize, sizeof(float));
	int j = 0;
	for( i = 0; i<nNumWins; i++ )
		if( bIsVoiced[i] )
			memcpy( lpRmv + para.SigP_RmvSilWinSz*(j++), fSignal + para.SigP_RmvSilWinSz * i, para.SigP_RmvSilWinSz * sizeof(float));

	free( bIsVoiced );
	free( fSignal );
	fSignal = lpRmv;
	nLen = nNewSize;	
	return nLen;
}

float CFeature::MeanEnergy(float *s, int nLen)
{
	double sum = 0.0;
	for( int i = 0; i<nLen ; i++ ){
		sum += s[i]*s[i];
	}
	return float (sum/nLen);
}

BOOL CFeature::IsSignalLoaded()
{
	return fSignal == NULL ? false : true;
}

float * CFeature::GetSig()
{
	return fSignal;
}

int CFeature::GetLen()
{
	return nLen;
}

short CFeature::RemoveDC()
{
	
	double sum = 0;
	for (int i=0; i<nLen; i++) sum += fSignal[i];

	float off = float(sum / nLen);
	for (i=0; i<nLen; i++)
		fSignal[i] -= off;

	return short(off);
}

CData * CFeature::ExtractEnergy()
{
	int NumWin = (nLen - para.WinSz) / para.StpSz + 1;
	CData *pEner = new CData( NumWin, 1);
	//float *logE = new float [NumWin];
	int i,j;
	float ener;
	
	//float* s = new float [para.WinSz];
	for (i = 0; i<NumWin; i++)
	{
		//memcpy( s, fSignal + i*para.StpSz, para.WinSz * sizeof(float) );
		//PreEmphasis( s, para.WinSz, float(para.Alpha) );
		//Window( s,para.WinSz );
		float *s = fSignal + i*para.StpSz;
		ener = 0;
		for (j = 1; j<= para.WinSz; j++ )
			ener += s[j]*s[j];
		//(pEner->fMat)[i][0] = log(ener);
		//logE[i] = float((ener < FLT_MIN) ? LZERO : log(ener));
	}

	//Normalization energy
	//float	max, min;
	
	/*// Tim thanh phan log E lon nhat 
	max = logE[0];
	for (i = 1; i < NumWin; i++)
	{
		if (logE[i] > max) max = logE[i];
	}

	min = float(max - (SILFLOOR * log(10.0)) / 10.0);  // Nguong thap nhat 
	// Chuan hoa ve khoang -Emin ... 1.0 
	for (i = 0; i < NumWin; i++)
	{
		if (logE[i] < min) logE[i] = min;
		(pEner->fMat)[i][0] = float(1.0 - (max - logE[i]) * ESCALE); 
	}

	delete logE;
	*/

	//free(s);
	return pEner;
}

void CFeature::Normalization(CData &data)
{
	if( data.nCol <= 0 ) return;
	//float *fMax = new float [data.nCol];

	int i,j;
	float temp;
	CData *p;

	for( i = 0; i<data.nRow; i++ )
	{
		//fMax[j] = 0;
		temp = 0;
		for( j=0; j<data.nCol; j++ )
			temp += (data.fMat)[i][j] * (data.fMat)[i][j];
		temp = float(sqrt( temp ));
		for( j=0; j<data.nCol; j++ )
			(data.fMat)[i][j] /= temp;
	}

	p = data.pNext;
	while( p != NULL )
	{
		for( i = 0; i<p->nRow; i++ )
		{
			//fMax[j] = 0;
			temp = 0;
			for( j=0; j<p->nCol; j++ )
				temp += (p->fMat)[i][j] * (p->fMat)[i][j];
			temp = float(sqrt( temp ));
			for( j=0; j<p->nCol; j++ )
				(p->fMat)[i][j] /= temp;
		}
		p = p->pNext;
	}
}
/*
void CFeature::Normalization(CData &data)
{
	if( data.nCol <= 0 ) return;
	//float *fMax = new float [data.nCol];

	int i,j;
	float temp,max;
	Data *p;

	max = 0;
	for( i = 0; i<data.nRow; i++ )
	{
		//fMax[j] = 0;
		temp = 0;
		for( j=0; j<data.nCol; j++ )
			temp += (data.fMat)[i][j] * (data.fMat)[i][j];
		temp = float(sqrt( temp ));
		if( max < temp )
			max = temp;
	}

	p = data.pNext;
	while( p != NULL )
	{
		for( i = 0; i<p->nRow; i++ )
		{
			//fMax[j] = 0;
			temp = 0;
			for( j=0; j<p->nCol; j++ )
				temp += (p->fMat)[i][j] * (p->fMat)[i][j];
			temp = float(sqrt( temp ));
			if( max < temp )
				max = temp;
		}
		p = p->pNext;
	}

	for( i = 0; i<data.nRow; i++ )
	{
		for( j=0; j<data.nCol; j++ )
			(data.fMat)[i][j] /= max;
	}

	p = data.pNext;
	while( p != NULL )
	{
		for( i = 0; i<p->nRow; i++ )
		{
			for( j=0; j<p->nCol; j++ )
				(p->fMat)[i][j] /= max;
		}
		p = p->pNext;
	}
}

for( i = 0; i<data.nRow; i++ )
	{
		//fMax[j] = 0;
		temp = 0;
		for( j=0; j<data.nCol; j++ )
			temp += (data.fMat)[i][j] * (data.fMat)[i][j];
		temp = sqrt( temp );
		for( j=0; j<data.nCol; j++ )
			(data.fMat)[i][j] /= temp;
	}*/

#define signum(i) (i < 0 ? -1 : i == 0 ? 0 : 1)
#define IRT2 0.707106781186548  // 1.0/sqrt(2.0) 


void FORD1(int m, float *b) {
    int j, k = 4, kl = 2, n = 0x1 << m;
    float t;
    
    for(j = 4; j <= n; j += 2) {
	if (k - j>0) {
	    t = b[j-1];
	    b[j - 1] = b[k - 1];
	    b[k - 1] = t;
	}
	k -= 2;
	if (k - kl <= 0) {
	    k = 2*j;
	    kl = j;
	}
    }	
}


void FORD2(int m, float *b) 
{
  float t;
  
  int n = 0x1<<m, k, ij, ji, ij1, ji1;
  
  int l[16], l1, l2, l3, l4, l5, l6, l7, l8, l9, l10, l11, l12, l13, l14, l15;
  int j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14;
  

  l[1] = n;
  for(k=2;k<=m;k++) l[k]=l[k-1]/2;
  for(k=m;k<=14;k++) l[k+1]=2;
  
  l15=l[1];l14=l[2];l13=l[3];l12=l[4];l11=l[5];l10=l[6];l9=l[7];
  l8=l[8];l7=l[9];l6=l[10];l5=l[11];l4=l[12];l3=l[13];l2=l[14];l1=l[15];

  ij = 2;
  
  for(j1=2;j1<=l1;j1+=2)
  for(j2=j1;j2<=l2;j2+=l1)
  for(j3=j2;j3<=l3;j3+=l2)
  for(j4=j3;j4<=l4;j4+=l3)
  for(j5=j4;j5<=l5;j5+=l4)
  for(j6=j5;j6<=l6;j6+=l5)
  for(j7=j6;j7<=l7;j7+=l6)
  for(j8=j7;j8<=l8;j8+=l7)
  for(j9=j8;j9<=l9;j9+=l8)
  for(j10=j9;j10<=l10;j10+=l9)
  for(j11=j10;j11<=l11;j11+=l10)
  for(j12=j11;j12<=l12;j12+=l11)
  for(j13=j12;j13<=l13;j13+=l12)
  for(j14=j13;j14<=l14;j14+=l13)
  for(ji=j14;ji<=l15;ji+=l14) {
    ij1 = ij-1; ji1 = ji - 1;
    if(ij-ji<0) {
      t = b[ij1-1];
      b[ij1-1]=b[ji1-1];
      b[ji1-1] = t;
	
      t = b[ij1];
      b[ij1]=b[ji1];
      b[ji1] = t;
    }
    ij += 2;
  }
} 

int fastlog2(int n) {
    int num_bits, power = 0;
    
    if ((n < 2) || (n % 2 != 0)) return(0);
    num_bits = sizeof(int) * 8;	
    
    while(power <= num_bits) {
	n >>= 1;
	power += 1;
	if (n & 0x01) {
	    if (n > 1)	return(0);
	    else return(power);
	}
    }
    return(0);
}


void FR2TR(int in, float *b0, float *b1) 
{
    int k;
    float t;
    for (k = 0; k < in; k++) 
    {
	t = b0[k] + b1[k];
	b1[k] = b0[k] - b1[k];
	b0[k] = t;
    }
}

void FR4TR(int in, int nn, float *b0, float *b1, float *b2, float* b3) {
  float arg, piovn, th2;
  float *b4 = b0, *b5 = b1, *b6 = b2, *b7 = b3;
  float t0, t1, t2, t3, t4, t5, t6, t7;
  float r1, r5, pr, pi;
  float c1, c2, c3, s1, s2, s3;
  
  int j, k, jj, kk, jthet, jlast, ji, jl, jr, int4;
  int L[16], L1, L2, L3, L4, L5, L6, L7, L8, L9, L10, L11, L12, L13, L14, L15;
  int j0, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14;
  int k0, kl;
  
  L[1] = nn / 4;	
  for(k = 2; k < 16; k++) {  
    switch (signum(L[k-1] - 2)) {
    case -1:
      L[k-1]=2;
    case 0:
      L[k]=2;
      break;
    case 1:
      L[k]=L[k-1]/2;
    }
  }

  L15=L[1]; L14=L[2]; L13=L[3]; L12=L[4]; L11=L[5]; L10=L[6]; L9=L[7];
  L8=L[8];  L7=L[9];  L6=L[10]; L5=L[11]; L4=L[12]; L3=L[13]; L2=L[14];
  L1=L[15];

  piovn = float(PI / nn);
  ji=3;
  jl=2;
  jr=2;
  
  for(j1=2;j1<=L1;j1+=2)
  for(j2=j1;j2<=L2;j2+=L1)
  for(j3=j2;j3<=L3;j3+=L2)
  for(j4=j3;j4<=L4;j4+=L3)
  for(j5=j4;j5<=L5;j5+=L4)
  for(j6=j5;j6<=L6;j6+=L5)
  for(j7=j6;j7<=L7;j7+=L6)
  for(j8=j7;j8<=L8;j8+=L7)
  for(j9=j8;j9<=L9;j9+=L8)
  for(j10=j9;j10<=L10;j10+=L9)
  for(j11=j10;j11<=L11;j11+=L10)
  for(j12=j11;j12<=L12;j12+=L11)
  for(j13=j12;j13<=L13;j13+=L12)
  for(j14=j13;j14<=L14;j14+=L13)
  for(jthet=j14;jthet<=L15;jthet+=L14) 
    {
      th2 = float(jthet - 2);
      if(th2<=0.0) 
	{
	  for(k=0;k<in;k++) 
	    {
	      t0 = b0[k] + b2[k];
	      t1 = b1[k] + b3[k];
	      b2[k] = b0[k] - b2[k];
	      b3[k] = b1[k] - b3[k];
	      b0[k] = t0 + t1;
	      b1[k] = t0 - t1;
	    }
	  if(nn-4>0) 
	    {
	      k0 = in*4 + 1;
	      kl = k0 + in - 1;
	      for (k=k0;k<=kl;k++) 
		{
		  kk = k-1;
		  pr = float(IRT2 * (b1[kk]-b3[kk]));
		  pi = float(IRT2 * (b1[kk]+b3[kk]));
		  b3[kk] = b2[kk] + pi;
		  b1[kk] = pi - b2[kk];
		  b2[kk] = b0[kk] - pr;
		  b0[kk] = b0[kk] + pr;
		}
	    }
	}
      else 
	{
	  arg = th2*piovn;
	  c1 = float(cos(arg));
	  s1 = float(sin(arg));
	  c2 = c1*c1 - s1*s1;
	  s2 = c1*s1 + c1*s1;
	  c3 = c1*c2 - s1*s2;
	  s3 = c2*s1 + s2*c1;

	  int4 = in*4;
	  j0=jr*int4 + 1;
	  k0=ji*int4 + 1;
	  jlast = j0+in-1;
	  for(j=j0;j<=jlast;j++) 
	    {
	      k = k0 + j - j0;
	      kk = k-1; jj = j-1;
	      r1 = b1[jj]*c1 - b5[kk]*s1;
	      r5 = b1[jj]*s1 + b5[kk]*c1;
	      t2 = b2[jj]*c2 - b6[kk]*s2;
	      t6 = b2[jj]*s2 + b6[kk]*c2;
	      t3 = b3[jj]*c3 - b7[kk]*s3;
	      t7 = b3[jj]*s3 + b7[kk]*c3;
	      t0 = b0[jj] + t2;
	      t4 = b4[kk] + t6;
	      t2 = b0[jj] - t2;
	      t6 = b4[kk] - t6;
	      t1 = r1 + t3;
	      t5 = r5 + t7;
	      t3 = r1 - t3;
	      t7 = r5 - t7;
	      b0[jj] = t0 + t1;
	      b7[kk] = t4 + t5;
	      b6[kk] = t0 - t1;
	      b1[jj] = t5 - t4;
	      b2[jj] = t2 - t7;
	      b5[kk] = t6 + t3;
	      b4[kk] = t2 + t7;
	      b3[jj] = t3 - t6;
	    }
	  jr += 2;
	  ji -= 2;
	  if(ji-jl <= 0) {
	    ji = 2*jr - 1;
	    jl = jr;
	  }
	}
    }
}

int CFeature::FastFFT(float *invec, int nLen)
{
	float fn;
    int i, in, nn, n2pow, n4pow, nthpo;
    
    int n = nLen;

    float *b = invec;

    n2pow = fastlog2(n);
    if (n2pow <= 0) 
	{
		ErrorMsg("Number of FFT must be pow of 2");
		return 0;
	}
    nthpo = n;
    fn = float(nthpo);
    n4pow = n2pow / 2;
    
    
    if (n2pow % 2) 
    {
		nn = 2;
		in = n / nn;
		FR2TR(in, b, b + in );
    }
    else 
		nn = 1;
    
    
    for(i = 1; i <= n4pow; i++) {
		nn *= 4;
		in = n / nn;
		FR4TR(in, nn, b, b + in, b + 2 * in, b + 3 * in);
    }
    
    
    FORD1(n2pow, b);
    FORD2(n2pow, b);
    
    
    for(i = 3; i < n; i += 2) b[i] = -b[i];    
    return 1;
}

CData * CFeature::Signal2FFT(const float *fSignal, const int nLen, bool use_power_rather_than_energy)
{
	int NumWin = (nLen - para.WinSz) / para.StpSz + 1;
	int i,half_fft_order;
    float real,imag;

    int fft_order;

	if( para.FFTSz < 0 )
	{
		fft_order = 2;   
		while (nLen > fft_order) 
		fft_order *= 2;
	}
	else 
	{
		fft_order = para.FFTSz;
		if( nLen > fft_order ) 
			ErrorMsg("Frame length must be smaller than FFTSz");
	}

	float *fft_invec = (float*) calloc( fft_order, sizeof(float) );
	memcpy( fft_invec, fSignal, sizeof(float) * nLen );
	FastFFT( fft_invec, fft_order );


	half_fft_order = fft_order/2;
	CData *pFFT = new CData(3, half_fft_order);
	float *fft_real = pFFT->fMat[1];
	float *fft_img = pFFT->fMat[2];
	float *fft     = pFFT->fMat[0];


    for(i=0;i<half_fft_order;i++)
    {
		real = fft_invec[i*2];
		imag = fft_invec[i*2+1];
	
		fft[i] = real * real + imag * imag;
		fft_img[i] = imag;
		fft_real[i] = real;
	}

    free(fft_invec);
    
	if( !use_power_rather_than_energy ) 
	{
		for(i=0;i<half_fft_order;i++)
			fft[i] = float(sqrt( fft[i] ));
	}

	return pFFT;
}

CData * CFeature::Signal2FFT()
{
	
	if( fSignal == NULL )
		ErrorMsg("You must load signal before : CopySignal");

	float *s = new float [para.WinSz];

	int NumWin = (nLen - para.WinSz) / para.StpSz + 1;
	
	CData *pFFT = NULL;
	CData *temp;
	
	for (int i = 0; i<NumWin; i++)
	{
		memcpy( s, fSignal + i*para.StpSz, para.WinSz * sizeof(float) );
		//if(para.PreEmphasis) 
		//	PreEmphasis( s,para.WinSz, float(para.Alpha));
		if(para.Windowing) 
			Window( s, para.WinSz );
		temp = Signal2FFT( s, para.WinSz );
		temp->nRow = 1;

		if( pFFT == NULL ) pFFT = new CData(NumWin, temp->nCol);
		memcpy( pFFT->fMat[i], temp->fMat[0], sizeof(float) * temp->nCol );
		delete temp;
	}

	delete[] s;
	return pFFT;		
}

void CFeature::PreEmphasis(float *sig, int nLen, float coef)
{
	float x = 0.0f;
    float x_1 = 0.0f;

	for (int i = 0; i < nLen; i++)
	{
	  x = sig[i];
	  sig[i] -= coef * x_1;
	  x_1 = x;
	}
	sig[0] *= (1-coef);
}

void CFeature::Hamming(float *sig, int nLen)
{
	GenHamWin( nLen );
	for ( int i = 0; i < nLen; i++ )
		sig[i] = fHamming->fWin[i]*sig[i];
}

void CFeature::GenHanWin(int nSize)
{
	if( fHanning->nSize == nSize ) return;
	fHanning->DeleteWin();
	fHanning->CreateWin(nSize);

	float *r_window = fHanning->fWin;
    int i,j=nSize/2;
    float k = float(PIx2 / nSize);

    if( (nSize & 1) != 0) // odd
		r_window[j]=1.0;

    for(i = 0; i < j; i++){
		r_window[i] = float(0.5f - 0.5f * cos(k * (i + 0.5f)));
		r_window[nSize-i-1] = r_window[i];
    }
}

void CFeature::Window(float *s, int nSize)
{
	float *fWin;
	switch(para.Windowing)
	{
		case HAMMING_WIN:
			GenHamWin( nSize );
			fWin = fHamming->fWin;
			break;
		case HANNING_WIN:
			GenHanWin( nSize );
			fWin = fHanning->fWin;
			break;
		case TRIANGLE_WIN:
			GenTriWin( nSize );
			fWin = fTriangle->fWin;
			break;
		default:
			return;
	};

	for ( int i = 0; i < nSize; i++ )
		s[i] = fWin[i]*s[i];
}

void CFeature::GenTriWin(int nSize)
{
	if( fTriangle->nSize == nSize ) return;
	fTriangle->DeleteWin();
	fTriangle->CreateWin(nSize);

	float *r_window = fTriangle->fWin;
    int i,j=nSize/2;
    float k = 2.0f / nSize;

    if( (nSize & 1) != 0) // odd
		r_window[j]=1.0;

    for(i = 0; i < j; i++){
		r_window[i] = i * k;
		r_window[nSize-i-1] = r_window[i];
    }
}

void CFeature::Hanning(float *sig, int nLen)
{
	GenHanWin( nLen );
	for ( int i = 0; i < nLen; i++ )
		sig[i] = fHanning->fWin[i]*sig[i];
}

void CFeature::Triangle(float *sig, int nLen)
{
	GenTriWin( nLen );
	for ( int i = 0; i < nLen; i++ )
		sig[i] = fTriangle->fWin[i]*sig[i];
}

static int slowFFTsub(float *real, int real_len, float *imag, int imag_len, float f) 
{


    float u_real,u_imag;
    float w_real,w_imag;
    float t_real,t_imag;
    float tmp_real,tmp_imag;

    int M,N;
    int i,j,k,l;
    
    M = fastlog2(real_len);
    N = (int)pow(2,(float)M);

    if (N != real_len)
      {
		//printf("Illegal FFT order %d", real_len);
		ErrorMsg("Illegal FFT order %d");
		return -1;
      }

    for(l=1;l<=M;l++){

	int le = (int)pow(2,(float)(M+1-l));
	int le1=le/2;

	u_real = 1.0;
	u_imag = 0.0;

	w_real= float(cos(PI/le1));
	w_imag=float(f * sin(PI/le1));

	for (j=1;j<=le1;j++)
	{
	    for (i=j;i<=N-le1;i+=le)
	    {
			int ip=i+le1;
			t_real = real[i-1] + real[ip-1];
			t_imag = imag[i-1] + imag[ip-1];

			tmp_real = real[i-1] - real[ip-1];
			tmp_imag = imag[i-1] - imag[ip-1];

			real[ip-1] = tmp_real*u_real - tmp_imag*u_imag;
			imag[ip-1] = tmp_real*u_imag + tmp_imag*u_real;

			real[i-1] = t_real;
			imag[i-1] = t_imag;
	    }

	    tmp_real = u_real*w_real - u_imag*w_imag;
	    tmp_imag = u_real*w_imag + u_imag*w_real;
	    
	    u_real=tmp_real;
	    u_imag=tmp_imag;

	}

    }


    int NV2=N/2;
    int NM1=N-1;
    j=1;


    for (i=1; i<=NM1;i++)
    {
	if (i < j)
	{
	    t_real=real[j-1];
	    t_imag=imag[j-1];
	    
	    real[j-1] = real[i-1];
	    imag[j-1] = imag[i-1];

	    real[i-1] = t_real;
	    imag[i-1] = t_imag;
	}

	k=NV2;

	while(k < j)
	{
	    j=j-k;
	    k=k/2;
	}

	j=j+k;

    }

    return 0;
}

int CFeature::IFFT(float *real, int real_len, float *imag, int imag_len)
{
    if (real_len <=0 )
	return -1;

    if (slowFFTsub( real, real_len, imag, imag_len, 1.0f) != 0)
	return -1;

    for(int i=1;i<=real_len;i++){
		real[i-1] /= real_len;
		imag[i-1] /= real_len;
    }

    return 0;
}

int CFeature::IFFT(CData &pFFT)
{
	if (( pFFT.fMat == NULL ) || ( pFFT.nRow < 2 )) 
		ErrorMsg( "pFFT contans at leat 2 row : real and imag");
	return IFFT( pFFT.fMat[0], pFFT.nCol, pFFT.fMat[1], pFFT.nCol );
}

CData * CFeature::DeltaRow(CData &DataSet)
{
	if( DataSet.fMat == NULL ) 
		ErrorMsg("Error in Variation : DataSet is empty");

	int i,k,j;
	float DeltaNumerator;
	int   DeltaDenominator;

	DeltaDenominator = 0;
	for (i = 1; i <= para.VariationStpSz; i++) DeltaDenominator += i*i;
	DeltaDenominator *= 2;

	/*float *lpBuf =  (float*)calloc( DataSet.nRow+(para.VariationStpSz <<1), sizeof(float) );
	CData *Delta;
	CData *pDelta = new CData();
	Delta = new CData;
	Delta->nCol = DataSet.nCol;
	Delta->nRow = DataSet.nRow;
	Delta->pNext = NULL;
	Delta->fMat = CData::New_FMatrix( DataSet.nRow, DataSet.nCol );

	for( i = 0; i < DataSet.nCol; i++ )
	{
		for (k = 0; k < DataSet.nRow; k++) lpBuf[k+para.VariationStpSz] = DataSet.fMat[k][i];
		for( k = 0; k < para.VariationStpSz; k++ )
		{
			lpBuf[k] = DataSet.fMat[0][i];
			lpBuf[ DataSet.nRow + k + para.VariationStpSz] = DataSet.fMat[DataSet.nRow - 1][i];
		}

		for (j = 0; j < DataSet.nRow; j++)
		{
			// Tinh tu so cua delta 
			DeltaNumerator = 0;
			for (k = 1; k <= para.VariationStpSz; k++)
				DeltaNumerator += k * (lpBuf[para.VariationStpSz + j + k] - lpBuf[para.VariationStpSz + j - k]);
			// Chia lay ket qua 
			Delta->fMat[j][i] = DeltaNumerator / DeltaDenominator;
		}
	}
	pDelta->Append(Delta);
	free( lpBuf );
*/

	CData* p = &DataSet;
	float *lpBuf;
	CData *Delta;
	CData *pDelta = new CData();
	while( p!=NULL )
	{
		lpBuf =  (float*)calloc( p->nRow+(para.VariationStpSz <<1), sizeof(float) );
		Delta = new CData;
		Delta->nCol = p->nCol;
		Delta->nRow = p->nRow;
		Delta->pNext = NULL;
		Delta->fMat = CData::New_FMatrix( p->nRow, p->nCol );

		for( i = 0; i < p->nCol; i++ )
		{
			for (k = 0; k < p->nRow; k++) lpBuf[k+para.VariationStpSz] = p->fMat[k][i];
			for( k = 0; k < para.VariationStpSz; k++ )
			{
				lpBuf[k] = p->fMat[0][i];
				lpBuf[ p->nRow + k + para.VariationStpSz] = p->fMat[p->nRow - 1][i];
			}

			for (j = 0; j < p->nRow; j++)
			{
				DeltaNumerator = 0;
				for (k = 1; k <= para.VariationStpSz; k++)
					DeltaNumerator += k * (lpBuf[para.VariationStpSz + j + k] - lpBuf[para.VariationStpSz + j - k]);
				Delta->fMat[j][i] = DeltaNumerator / DeltaDenominator;
			}
		}
		pDelta->Append(Delta);
		free( lpBuf );
		p = p->pNext;
	}

	return pDelta;
}

static float compute_gradient(float *x, int x_len, int num_points)
{
    float gradient;
    
    switch (num_points){
	
    case 1:
	gradient = 0.0;
	break;
	
    case 2:
	gradient = x[0] - x[1];
	break;
	
    case 3:
	gradient = (x[0] -x[2]) / 2.0f;
	break;
	
    case 4:
	gradient = ( (3.0f*x[0]) + x[1] - x[2] - (3.0f * x[3]) ) / 10.0f;
	break;
	
    default:
	
	cerr << "compute_gradient(float*, int) : ERROR : num_points is" 
	    << num_points << endl;
	
	ErrorMsg("Error in compute_gradient");
	break;
    }
    
    return gradient;
}


# define MAX_REGRESSION_LENGTH 4
CData * CFeature::DeltaCol(CData &DataSet)
{
	int reg_index, this_index;
    para.VariationStpSz *= 2;
    // need at least two points to compute gradient
    if ((para.VariationStpSz < 2)||(para.VariationStpSz > MAX_REGRESSION_LENGTH)){
	cerr << "delta(EST_Track&, int) : ERROR : regression_length is "
	    << para.VariationStpSz << endl;
	ErrorMsg("Error in Delta2");
    }

	CData *pDelta = new CData();
	CData *pTempDel;
  

    float *temp = new float [para.VariationStpSz];
    
	CData *Cur = &DataSet;
	while( Cur != NULL )
	{
		pTempDel = new CData( Cur->nRow, Cur->nCol );
    for (int j = 0; j < Cur->nRow; j++ )
	{

	for (int i = 0; i < Cur->nCol; i++)
	{

	    for (reg_index=0; reg_index<para.VariationStpSz; reg_index++)
	    {
		this_index = i - reg_index;
		if (this_index >= 0)
		    temp[reg_index] = Cur->fMat[j][this_index];
	    }
	    

	    if (i < 1)
		pTempDel->fMat[j][i] = 0.0;
	    else if (i < para.VariationStpSz - 1)
		pTempDel->fMat[j][i] = compute_gradient( temp, para.VariationStpSz, i + 1);
	    else

		pTempDel->fMat[j][i] = compute_gradient( temp, para.VariationStpSz, para.VariationStpSz);
	}

	}
		pDelta->Append( pTempDel );
	Cur = Cur->pNext;
	}
	delete[] temp;
	para.VariationStpSz /= 2;
	return pDelta;
}

int slowIFFT(FilterWin &real, FilterWin &imag) 
{
    int N = real.nSize;
    if (N <=0 )
	return -1;

    if ( slowFFTsub( real.fWin, real.nSize, imag.fWin, imag.nSize,1.0) != 0)
	return -1;

    for(int i=1;i<=N;i++){
	real[i-1] /= (float)N;
	imag[i-1] /= (float)N;
    }

    return 0;
}

FilterWin *design_FIR_filter(const FilterWin &frequency_response, 
			      int filter_order)
{

    if((filter_order & 1) == 0){
		ErrorMsg("Requested filter order must be odd");
    }
	

    int N = fastlog2( frequency_response.nSize);
    if(frequency_response.nSize !=  (int)pow(2,(float)N))
	{
		ErrorMsg("Desired frequency response must have dimension 2^N");
    }

    int i;
	FilterWin filt;
	filt.CreateWin(frequency_response.nSize);
	memcpy( filt.fWin, frequency_response.fWin, sizeof(float) * filt.nSize );

    FilterWin dummy;
	dummy.CreateWin(frequency_response.nSize);
    for(i=0;i<dummy.nSize;i++)
		dummy.fWin[i] = 0.0;

    int e = slowIFFT(filt,dummy);
    if (e != 0)
    {
		ErrorMsg("Failed to design filter because FFT failed");
    }

    FilterWin *reduced_filt = new FilterWin;
	reduced_filt->CreateWin(filter_order);

    int mid = filter_order/2;

    reduced_filt->fWin[mid] = filt[0];
    for(i=1; i<=mid ;i++)
    {

	float window =  float(0.5 + 0.5 * cos(PI*i /mid));
		reduced_filt->fWin[mid+i] = filt.fWin[i] * window;
		reduced_filt->fWin[mid-i] = filt.fWin[i] * window;
    }
	filt.DeleteWin();
	dummy.DeleteWin();

    return reduced_filt;
}

FilterWin *design_high_or_low_pass_FIR_filter(int sample_rate, 
					       int cutoff_freq, int order,
					       float gain1, float gain2)
{
    
    if (sample_rate <= 0){
		ErrorMsg("Can't design a FIR filter for a sampling rate of <=0");
    }
    
    int i;
    int N=10;			
    
    int fft_size = (int)pow(2, N);
    while(fft_size < order*4){	
	N++;
	fft_size = (int)pow(2, N);
    }
    
    
    FilterWin freq_resp;
	freq_resp.CreateWin(fft_size);

    int normalised_cutoff = (fft_size * cutoff_freq)/sample_rate;
    for(i=0;i<normalised_cutoff;i++)
	{
		freq_resp.fWin[i] = gain1;
		freq_resp.fWin[fft_size-i-1] = gain1;
    }

    for(;i<fft_size/2;i++){
		freq_resp.fWin[i] = gain2;
		freq_resp.fWin[fft_size-i-1] = gain2;
    }
	FilterWin *temp = design_FIR_filter(freq_resp, order);
	freq_resp.DeleteWin();
    
    return temp;
}

float *FIRfilter(const float *in_sig,const int sig_len,
	       const float* numerator, const int num_len, int delay_correction)
{
    if (delay_correction < 0)
		ErrorMsg("Can't have negative delay !\n");

    if (num_len <= 0)
		ErrorMsg("Can't filter EST_Wave with given filter");

    int i,j,n= sig_len;
	float *out_sig = (float*)calloc(n, sizeof(float));
	if( out_sig == NULL )  
		ErrorMsg("Not enough memory in FIRfilter");

    for (i = 0; i < n; ++i)
    {
		out_sig[i] = 0;

		int jlow=0;
		int jhigh=num_len;

		if(i+delay_correction >= n)
			jlow = i + delay_correction - n + 1;

		if(i+delay_correction - jhigh < 0)
			jhigh = i + delay_correction;

		for(j=jlow; j<jhigh; j++)
			if (((i+delay_correction - j) >= 0) &&	((i+delay_correction - j) < n))
			out_sig[i] += in_sig[i+delay_correction - j] * numerator[j];
    }
	return out_sig;
}

float * CFeature::FIRlowpass_filter(float *sig, int nLen, int nSampleRat, int nCutOffFreq, int nOrder)
{
	FilterWin *filt = design_high_or_low_pass_FIR_filter(nSampleRat, nCutOffFreq, nOrder, 1.0, 0.0);
	float *out_sig = FIRfilter( sig, nLen, filt->fWin, filt->nSize, (filt->nSize)/2);
	filt->DeleteWin();
	delete filt;
	return out_sig;
}

void CFeature::FIRlowpass_filter()
{
	float *out_sig = FIRlowpass_filter(fSignal,nLen,para.SRate,para.LowFilCutOffFreq, para.FilOrder );
	memcpy( fSignal, out_sig, nLen * sizeof(float));
	delete[] out_sig;
}

void CFeature::FIRlowpass_double_filter()
{
	float *out_sig = FIRlowpass_double_filter(fSignal,nLen,para.SRate,para.LowFilCutOffFreq, para.FilOrder );
	memcpy( fSignal, out_sig, nLen * sizeof(float));
	delete[] out_sig;
}

void reverse(float *sig, int nLen)
{
    int i, n;
    float t;
    n = (int)(nLen/2.0f);
    
    for (i = 0; i < n; ++i)
    {
		t = sig[i];
		sig[i] = sig[nLen - 1 - i];
		sig[nLen - 1 -i] = t;
    }
}

float * CFeature::FIRlowpass_double_filter(float *sig, int nLen, int nSampleRate, int nCutOffFreq, int nOrder)
{
	FilterWin *filt = design_high_or_low_pass_FIR_filter(nSampleRate, nCutOffFreq, nOrder, 1.0, 0.0);
    
	float *out1_sig = FIRfilter( sig, nLen, filt->fWin, filt->nSize, (filt->nSize)/2);
	reverse( out1_sig, nLen );
	float *out2_sig = FIRfilter( out1_sig, nLen, filt->fWin, filt->nSize, (filt->nSize)/2);
	reverse( out2_sig, nLen );

	filt->DeleteWin();
	delete filt;
	delete out1_sig;
	return out2_sig;
}

float * CFeature::FIRhighpass_double_filter(float *sig, int nLen, int nSampleRate, int nCutOffFreq, int nOrder)
{
	FilterWin *filt = design_high_or_low_pass_FIR_filter( nSampleRate, nCutOffFreq, nOrder, 0.0f, 1.0f);
	float *out1_sig = FIRfilter( sig, nLen, filt->fWin, filt->nSize, (filt->nSize)/2);
	reverse( out1_sig, nLen );
	float *out2_sig = FIRfilter( out1_sig, nLen, filt->fWin, filt->nSize, (filt->nSize)/2);
	reverse( out2_sig, nLen );

	filt->DeleteWin();
	delete filt;
	delete out1_sig;
	return out2_sig;
}

void CFeature::FIRhighpass_double_filter()
{
	float *out_sig = FIRhighpass_double_filter(fSignal,nLen,para.SRate,para.HighFilCutOffFreq, para.FilOrder );
	memcpy( fSignal, out_sig, nLen * sizeof(float));
	delete[] out_sig;
}

void CFeature::FIRhighpass_filter()
{
	float *out_sig = FIRhighpass_filter(fSignal,nLen,para.SRate,para.HighFilCutOffFreq, para.FilOrder );
	memcpy( fSignal, out_sig, nLen * sizeof(float));
	delete[] out_sig;
}

float * CFeature::FIRhighpass_filter(float *sig, int nLen, int nSampleRate, int nCutOffFreq, int nOrder)
{
	FilterWin *filt = design_high_or_low_pass_FIR_filter( nSampleRate, nCutOffFreq, nOrder, 0.0f, 1.0f);    
	float *out_sig = FIRfilter( sig, nLen, filt->fWin, filt->nSize, (filt->nSize)/2);
	filt->DeleteWin();
	delete filt;
	return out_sig;
}

void CFeature::Simple_Mean_Smooth(float *sig, int nSize, int nOrder)
{
    int i, j, h, k=1;
    float *a = new float[nSize];
    float sum;
    h = nOrder/2;
    
    for (i = 0; (i < h); ++i)
    {
		k = (i * 2) + 1;
		sum = 0.0f;
		for (j = 0; (j < k) && (k < nSize); ++j)
			sum += sig[j];
		a[i] = sum / k;
    }
    
    for (i = h; i < nSize - h; ++i)
    {
		sum = 0.0f;
		for (j = 0; j < nOrder; ++j)
			sum += sig[i - h + j];
		a[i] = sum / k;
    }
    
    for (; i < nSize; ++i)
    {
		k = ((nSize - i)* 2) -1;
		sum = 0.0f;
		for (j = 0; j < k; ++j)
			sum += sig[i - (k/2) + j];
		a[i] = sum / k;
    }
    
    for (i = 0; i < nSize; ++i)
	sig[i] = a[i] + 0.5f;
    
    delete [] a;
}

void CFeature::NormalizeCol(CData &data)
{
	CData *p = &data;
	float max = 0.0f;
	while( p!=NULL)
	{
		for( int j=0 ; j<p->nCol; j++ )
		{
			for( int i = 0; i< p->nRow; i++ )
			{
				if( max < float(fabs(p->fMat[i][j])) )
					max = p->fMat[i][j];
			}
			if( max == 0.0f ) max = 1.0f;
			for( i = 0; i< p->nRow; i++ )
			{
				p->fMat[i][j] /= max;
			}
		}

		p = p->pNext;
	}
}
